#include "publics.h"


using namespace Weldroid;
double RobotPos::dis(RobotPos p) /// 间距
{
    return sqrt(pow(x-p.x,2)+pow(y-p.y,2)+pow(z-p.z,2));
}

RobotPos RobotPos::instance(string str)
{
    RobotPos p = {0,0,0,0,0,0};
    if(str == "")
        return p;
    char split[3] = {',',' ','\t'};
    std::vector<double> ds;
    for(int i=0;i<3;i++)
    {
        ds.resize(0);
        ds.clear();
        stringstream ss(str);
        while(ss.good())
        {
            string substr;
            getline( ss, substr,split[i]);
            stringstream sd(substr);
            double di;
            sd>>di;
            ds.push_back(di);
        }
        if(ds.size() >= 6)
            break;
    }
    if(ds.size() < 6)
        return p;
    p.x = ds[0];
    p.y = ds[1];
    p.z = ds[2];
    p.a = ds[3];
    p.b = ds[4];
    p.c = ds[5];
    p.v = 0;
    p.acc = 80;
    return p;
}



